#ifndef FASTLOOP_H
#define FASTLOOP_H

#include <common/Runnable.h>
#include <common/filter.h>
#include <common/array.h>
#include <common/orbhelper.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/zorfcs_state.h>
#include <uORB/topics/sdzorlog_status.h>
#include <uORB/topics/engine.h>


class ControlLoop;
class ParamManager;
class InputPin;
class RawDataFilter;

class FastLoop : public Runnable {
public:
	FastLoop(ParamManager *parammgr, InputPin *wow = NULL);
	virtual ~FastLoop();
	
private:
	virtual void run();
	void init();
	void createControlLoops();
	void destroyControlLoops();
	void doControlLoops() const;	
	void updateAirspeedScale() const;
	void updateFBWEnabled() const;
	void throttleIdleCorrection() const;
	void updateWow();
	void updateEngine();
	void updateLaunchAccel();

	void readOrbToProperties();
	void writePropertiesToOrb();

	ParamManager *parammgr_;
	Array<ControlLoop *> loops_;
	uint32_t writeCounter_;
	float throttle_idle_;
	float vfbw_;
	float vrotate_;
	InputPin *wow_;
	Filter<bool> *wowFilter_;
	Filter<bool> *engineFilter_;
	Filter<bool> *accelFilter_;
	RawDataFilter *rawDataFilter_;

	orbhelper::OrbSubscriber<sensor_combined_s, ORB_ID(sensor_combined)> sensor_combined_;
	orbhelper::OrbSubscriber<vehicle_attitude_s, ORB_ID(vehicle_attitude)> vehicle_attitude_;
	orbhelper::OrbSubscriber<vehicle_gps_position_s, ORB_ID(vehicle_gps_position)> vehicle_gps_position_;
	orbhelper::OrbSubscriber<vehicle_global_position_s, ORB_ID(vehicle_global_position)> vehicle_global_position_;
	orbhelper::OrbPublisher<actuator_controls_s, ORB_ID_VEHICLE_ATTITUDE_CONTROLS> actuator_controls_;
	orbhelper::OrbSubscriber<rc_channels_s, ORB_ID(rc_channels)> rc_channels_;
	orbhelper::OrbSubscriber<manual_control_setpoint_s, ORB_ID(manual_control_setpoint)> manual_control_setpoint_;
	orbhelper::OrbSubscriber<airspeed_s, ORB_ID(airspeed)> airspeed_;
	orbhelper::OrbPublisher<zorfcs_state_s, ORB_ID(zorfcs_state)> zorfcs_state_;
	orbhelper::OrbSubscriber<sdzorlog_status_s, ORB_ID(sdzorlog_status)> sdzorlog_status_;
	orbhelper::OrbSubscriber<engine_s, ORB_ID(engine)> engine_;
};

#endif
